#include "ros/ros.h"
#include "std_msgs/String.h"
#include "std_msgs/Int32.h"
#include "std_msgs/Empty.h"
#include "tf/transform_datatypes.h"
#include "sensor_msgs/Imu.h"
#include "geometry_msgs/Twist.h" 
#include "nlink_parser/LinktrackAnchorframe0.h"
#include "nlink_parser/LinktrackTagframe0.h"
#include "tello_driver/TelloStatus.h"
#include <math.h>
#include <iostream>
#define PI 3.1415926

using namespace std;

int Num = 30; // 30 UAVs
double tello_pos[30][3]; // 30 UAV x 3D positions


vector<ros::Publisher> pos_pubs;
vector<ros::Publisher> takeoff_pubs;
vector<ros::Publisher> land_pubs;

int command = 0;

bool already_takeoff = false;
bool already_land = true;

int takeoff_cmd_id = 0;
int land_cmd_id = 0;

/* read UWB positions from ROS topic (only x and y pos)*/
void cb_readPos(const nlink_parser::LinktrackAnchorframe0::ConstPtr& msg)
{
	for(auto &item : msg->nodes)
		{
			if(1 <= item.id && item.id <= Num)
				{
				tello_pos[item.id-1][0] =  item.pos_3d[0]; // x
				tello_pos[item.id-1][1] =  item.pos_3d[1]; // y
				// tello_pos[item.id-1][2] =  item.pos_3d[2]; // z
				}
			else
			{
				// cout << "unrecognized uwb id: " << item.id << endl;
			}
		}
}

/* Read command from ROS topic */
void cb_command(const std_msgs::Int32::ConstPtr& msg)
{
	std_msgs::Int32 cmd_data;
	cmd_data = *msg;
	command = cmd_data.data;
	cout << "receive command: " << command << endl;
	if (command == 1)
	{
		takeoff_cmd_id ++;
	}
	else if(command == 9)
	{
		land_cmd_id ++;
	}
	else
	{
		cout << "Invalid command: " << command << endl;
	}
}

/* Read tello number to land it seperately */
void cb_land_num(const std_msgs::Int32::ConstPtr& msg)
{
	std_msgs::Int32 land_cmd_data;
	land_cmd_data = *msg;
	int number;
	number = land_cmd_data.data;
	if(number <= Num && number >=1)
	{
		for (int i_land = 1; i_land<=10; i_land++) // loop for 10 times
		{
			std_msgs::Empty msg;
			land_pubs[number-1].publish(msg);
		}
	}
	else
	{
		cout << "[error] Land number: " << number << " should be within [1," << Num << "]" << endl;
	}
}

/* Read tello number to takeoff it seperately */
void cb_takeoff_num(const std_msgs::Int32::ConstPtr& msg)
{
	std_msgs::Int32 takeoff_cmd_data;
	takeoff_cmd_data = *msg;
	int number;
	number = takeoff_cmd_data.data;
	if(number <= Num && number >=1)
	{
		for (int i_takeoff = 1; i_takeoff<=10; i_takeoff++) // loop for 10 times
		{
			std_msgs::Empty msg;
			takeoff_pubs[number-1].publish(msg);
		}
	}
	else
	{
		cout << "[error] takeoff number: " << number << " should be within [1," << Num << "]" << endl;
	}
}


int main(int argc,char **argv)
{
	ros::init(argc,argv,"tello0");
	ros::NodeHandle nh("");

	// subscribers for command and UWB positions
	ros::Subscriber cmd_sub = nh.subscribe("/command",10,cb_command);
	ros::Subscriber land_sub = nh.subscribe("/land_num",10,cb_land_num);
	ros::Subscriber takeoff = nh.subscribe("/takeoff_num",10,cb_takeoff_num);
	ros::Subscriber pos_sub = nh.subscribe("/nlink_linktrack_anchorframe0",10,cb_readPos); //cb means Callback

	// publisher for tello positions
	for(int i=0; i<Num; i++)
	{
		ros::Publisher publisher = nh.advertise<geometry_msgs::Twist>("/tello"+to_string(i+1)+"/pos",10); 
		pos_pubs.emplace_back(publisher);
	}

	//publisher for tello takeoff command
	for(int i=0; i<Num; i++)
	{
		ros::Publisher publisher = nh.advertise<std_msgs::Empty>("/tello"+to_string(i+1)+"/takeoff",10); 
		takeoff_pubs.emplace_back(publisher);
	}

	//publisher for tello land command
	for(int i=0; i<Num; i++)
	{
		ros::Publisher publisher = nh.advertise<std_msgs::Empty>("/tello"+to_string(i+1)+"/land",10); 
		land_pubs.emplace_back(publisher);
	}

	int last_takeoff_cmd_id = 0;
	int last_land_cmd_id = 0;

	ros::Rate loop_rate(50); // 50Hz


	/* Main Loop */
	while(ros::ok())
	{	
		/* publish UWB positions to ROS topics */
		for(int i=0; i<Num; i++)
		{
			geometry_msgs::Twist msg;
			msg.linear.x = tello_pos[i][0];
			msg.linear.y = tello_pos[i][1];
			msg.linear.z = tello_pos[i][2];
			pos_pubs[i].publish(msg);
		}

		/* process takeoff and land command */
		if(command == 1) // takeoff
		{
			if(already_takeoff == false || last_takeoff_cmd_id != takeoff_cmd_id)
			{	// if have not taken off  or new takeoff command is receicved
				cout<<"qifei"<<endl;
				for (int i_tf = 1; i_tf<=5; i_tf++) // loop for 5 times
				{
					for (int j=0; j<Num; j++)
					{
						std_msgs::Empty msg;
						takeoff_pubs[j].publish(msg);
					}
					ros::Duration(0.1).sleep();
				}
				already_takeoff = true;
				already_land = false;
			}
		}	

		// land
		if(command == 9)
		{
			if(already_land == false || last_land_cmd_id != land_cmd_id)
			{	// if have not landed  or new landing command is receicved
				cout<<"jiangluo"<<endl;
				for (int i_land = 1; i_land<=20; i_land++) // loop for 20 times
				{
					for (int j=0; j<Num; j++)
					{
						std_msgs::Empty msg;
						land_pubs[j].publish(msg);
					}
					ros::Duration(0.05).sleep();
				}
				already_land = true;
				already_takeoff = false;
			}
		}	

		last_takeoff_cmd_id = takeoff_cmd_id;
		last_land_cmd_id = land_cmd_id;

		ros::spinOnce();
		loop_rate.sleep();
	}  

	return 0;
}
